package behavior;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.navigation.TachoPilot;
/**
 * An object of this class is used to detect the panel, the robot is on. It uses the LightSensor.
 * 
 * @author Simon & Maarten
 *
 */
public class LineAnalyser extends Analyser {
	private LightSensor sensor;
	private TachoPilot pilot;
	private int straightDistance;
	private int color;
	private float distance;
	private int brownValue;
	private int whiteMargin=20;
	private int blackMargin=25;
	
	public LineAnalyser(TachoPilot pilot){
		super(pilot);
		this.sensor=new LightSensor(SensorPort.S1);
		this.pilot = pilot;
		
		straightDistance=30;
	}
	/**
	 * This method is used to detect which panel the robot is on. 
	 * The robot drives forward until he reaches a predefined distance (the length of a panel)
	 * or until he finds a line (method lineFound). 
	 * When he has driven the predefined distance and hasn't found a line, the panel is a
	 * straight one. 
	 * When a line is found, depending on the color of the line, the robot takes a 90� turn to
	 * the left or the right.
	 */
	public int analyse(){
		brownValue=sensor.getNormalizedLightValue();
		pilot.reset();
		pilot.forward();
		distance=0;
		color=sensor.getNormalizedLightValue();
		
		
		while(distance<straightDistance && !lineFound()){
			color=sensor.getNormalizedLightValue();
			distance=pilot.getTravelDistance();
			System.out.println(distance);
		}
		pilot.stop();
		if(distance>=straightDistance){
			sleep(100);
			pilot.travel(-distance);
			//doorgeven dat je op straight stuk zit
			return 1;
		}
		else {
			if(getValue(color)==1){
				sleep(100);
				pilot.travel(-distance);
				//rechts doorgeven
				return 6;
			}
			else{
				sleep(100);
				pilot.travel(-distance);
				//links doorgeven
				return 3;
			}
		}
			
		//System.out.println(distance);
	
	}
	
	private boolean lineFound(){
		return getValue(color)!=2;
	}
	
	private int getValue(int color){
		if (color > brownValue+whiteMargin){
			return 1;
		}
		if(color<brownValue-blackMargin){
			return 0;
		}
		else{
			return 2;
		}
	}
	
	public void ExecuteTestReadings() {

		LCD.drawString("executing testreadings", 0, 1);
		while (!Button.ENTER.isPressed()) {

			int reading = sensor.getNormalizedLightValue();
			System.out.println(reading);
			

		}
		LCD.clear();
	}
	
	public void test(){
		while(true){
			Button.waitForPress();
			System.out.println(analyse());
		}
	}
	
}
